#include <ros/ros.h>
#include <fstream>
#include <iostream>
#include <tf/transform_datatypes.h>
#include <iostream>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
// #include "mission_utils.h"
// #include "message_utils.h"
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <geometry_msgs/PoseStamped.h>
#include <kcf_tracker/kcfTrackerRoi.h>

using namespace std;
using namespace Eigen;
using namespace message_filters;

std::ofstream outfile;

void callback(const geometry_msgs::PoseStamped::ConstPtr &msg1, const geometry_msgs::PoseStamped::ConstPtr &msg2, const kcf_tracker::kcfTrackerRoi::ConstPtr &msg3)
{
    auto time = ros::Time::now().toNSec()/1000000000.0;
    outfile  << setiosflags(ios::fixed) << setprecision(7) 
        << time << "\t"
        << msg1->pose.position.x << "\t" << msg1->pose.position.y << "\t" << msg1->pose.position.z << "\t"
        << msg2->pose.position.x << "\t" << msg2->pose.position.y << "\t" << msg2->pose.position.z << "\t" << msg2->pose.orientation.x << "\t" << msg2->pose.orientation.y << "\t" << msg2->pose.orientation.z << "\t" << msg2->pose.orientation.w << "\t"
        << msg3->x << "\t" << msg3->y << "\t" << msg3->width << "\t" << msg3->height << "\t" << msg3->distance << "\t"
        << std::endl;
    std::cout << "synchronized!!    " << setiosflags(ios::fixed) << setprecision(7)  << time << std::endl;
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "data_saver");
    ros::NodeHandle nh("~");

    string saveFilePath = "/home/nvidia/work/dataset/saveFile/"; 
    // 获取当前时间，精确到秒
    time_t currentTime = std::time(NULL);
    char chCurrentTime[64];
    std::strftime(chCurrentTime, sizeof(chCurrentTime), "%Y-%m-%d-%H-%M-%S", std::localtime(&currentTime)); //年月日 时分秒
    std::string stCurrentTime = chCurrentTime;// 转为string

    outfile.open(saveFilePath + "outfile" + "-"  + stCurrentTime+ ".csv" , ios::trunc);

    if(!outfile.is_open())
    {
        cout << "fail to open!" << endl;
        return -1;
    }
    message_filters::Subscriber<kcf_tracker::kcfTrackerRoi> uav1_det_sub(nh, "/kcfTracker/result/trackRoi", 1);
    message_filters::Subscriber<geometry_msgs::PoseStamped> uav1_Pose_sub(nh, "/vrpn_client_node/px4_q250/pose", 1);
    message_filters::Subscriber<geometry_msgs::PoseStamped> tb_Pose_sub(nh, "/vrpn_client_node/turtlebot1/pose", 1);


    typedef sync_policies::ApproximateTime< geometry_msgs::PoseStamped, geometry_msgs::PoseStamped, kcf_tracker::kcfTrackerRoi> MySyncPolicy;
    Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), tb_Pose_sub, uav1_Pose_sub, uav1_det_sub);
    sync.registerCallback(boost::bind(callback, _1, _2, _3));
    ros::spin();
    return 0;
}
